# -- coding:utf-8 --
#!/usr/bin/python
import sys, os
import inspect
from SnakeRobot import SnakeRobot
from BwRobotLib import BwRobotLib
from MiniBotControlLib import MiniBotControlLib
import time
import BwHelpLib 
import _thread

msg = """
键盘控制命令 :
w : 前进
x : 后退
s : 停止
a : 左转
d : 右转
"""

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath = os.path.dirname(os.path.abspath(fileName))
config_file = folderPath+"/config.txt"

class SnakeRobotClass:
    def __init__(self):
        # 实例化MiniBotControlLib类，并设置为udp模式
        self.mlib = MiniBotControlLib("udp")
        # 输入ip地址和端口号
        self.ip = "192.168.1.168"
        self.port = 50000
        # 连接机器人,并设置超时时间2s
        self.mlib.Connect(self.ip, self.port, 2000)   
        # 实例化BwRobotLib类，并将mlib作为参数传入
        self.robotlib = BwRobotLib(self.mlib)
        # 导入配置文件，获取文件中的模块ID和模块连接位置
        self.module_ids, self.module_ids_loc = self.robotlib.loadConfig(config_file)
        # 扫描当前机器人构型的ID，默认扫描ID段为20-50
        self.module_ids_scan, self.module_ids_loc_scan = self.mlib.ScanId()
        # 判断是否扫描的ID与配置文件导入的ID一致
        res = BwHelpLib.checkListEqual(self.module_ids,self.module_ids_scan)
        if res == False:
            print("\033[31m输入的模块ID与扫描获取的模块ID不一致，请检查！\033[37m")
            print("配置文件中模块ID: ", self.module_ids)
            print("实际扫描的模块ID: ", self.module_ids_scan)
            exit(0)
        else:
            print("输入的模块ID与扫描获取的模块ID一致")
            
        # 实例化操作臂类
        self.snakeRobot = SnakeRobot(self.mlib)
        # 将模块ID与操作臂构型绑定
        self.snakeRobot.bindIds(self.module_ids)        


    def handleControlKeys(self, cmd):
        
        if not(cmd == '6' or cmd == '7' or cmd == '8' or cmd == '3'):
            if not (self.snake.stopMoving_signal):
                return

        if(cmd == 'w'):
            self.snake.goForward()
        if(cmd == 'x'):
            self.snake.goBack()
        if(cmd == 's'):
            self.snake.stop()
        if(cmd == 'a'):
            self.snake.turnLeft()
        if(cmd == 'd'):
            self.snake.turnRight()

        if(cmd == '6' or cmd == '7' or cmd == '8'):
            self.handleSettingCmd(cmd)
        pass

    def handleSettingCmd(self, cmd):
        print("请输入参数并回车:")
        parameter = BwHelpLib.inputNum()
    
        if(cmd == '6'):
            self.snake.setVelocity(parameter)
        if(cmd == '7'):
            self.snake.setSwingRange(parameter)
        if(cmd == '8'):
            self.snake.setSwingHz(parameter)

    #运行线程
    def snakeRun(self,threadName,arg):
        self.snakeRobot.run()

cmd_msg = """
命令：
1 : 控制
2 : 设置速率参数
3 : 设置振幅
4 : 设置频率
-----------------------
0 : 退出
"""
def main():
    # print("输入任意字符 Enter 开始初始化...")
    snake_robot = SnakeRobotClass()
    snake_robot.snakeRobot.stop()
    _thread.start_new_thread(snake_robot.snakeRun,("runthread",snake_robot.snakeRobot))
    while(1):
        print(cmd_msg)
        cmd = BwHelpLib.inputNum(int,"请输入：")
        if(cmd == 1):
            print(msg)
        if(cmd == 2):
            snake_robot.handleSettingCmd(6)
        if(cmd == 3):
            snake_robot.handleSettingCmd(7)
        if(cmd == 4):
            snake_robot.handleSettingCmd(8)
        if(cmd == 0):
            break
if __name__ == "__main__":
    try :
        main()    
    except KeyboardInterrupt:
        print(" Program closed! ")
    pass

